
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_channel.c
  * @author     baiyang
  * @date       2022-11-16
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rc_channel.h"

#include <stdlib.h>

#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
#include <mission/mb_mission.h>
#include <common/time/gp_time.h>
#include <mb_arming/mb_arming.h>
#include <srv_channel/srv_channel.h>
#include <board_config/board_config.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define SWITCH_DEBOUNCE_TIME_MS  200
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rc_ctor(rc_channel_t rc, rc_ops_t ops)
{
    rc->ops = ops;

    rc->switch_state.debounce_position = -1;
    rc->switch_state.current_position  = -1;
}

// read input from hal.rcin - create a control_in value
bool rc_update(rc_channel_t rc)
{
    if (rc_has_override(rc) && !rcs_ignore_overrides()) {
        rc->radio_in = rc->override_value;
    } else if (rcs_has_had_rc_receiver() && !rcs_ignore_receiver()) {
        rc->radio_in = (int16_t)rc_hal_read(rc->ch_in);
    } else {
        return false;
    }

    if (rc->type_in == RC_CONTROL_TYPE_RANGE) {
        rc->control_in = rc_pwm_to_range(rc);
    } else {
        // ControlType::ANGLE
        rc->control_in = rc_pwm_to_angle(rc);
    }

    return true;
}

void rc_set_range(rc_channel_t rc, uint16_t high)
{
    rc->type_in = RC_CONTROL_TYPE_RANGE;
    rc->high_in = high;
}

void rc_set_angle(rc_channel_t rc, uint16_t angle)
{
    rc->type_in = RC_CONTROL_TYPE_ANGLE;
    rc->high_in = angle;
}

void rc_set_default_dead_zone(rc_channel_t rc, int16_t dzone)
{
    param_t* dead_zone = param_get_by_variable2("RC", &rc->dead_zone);

    if (dead_zone != NULL) {
        param_set_obj(dead_zone, abs(dzone));
    } else {
        rc->dead_zone = abs(dzone);
    }
}

bool rc_get_reverse(rc_channel_t rc)
{
    return (bool)rc->reversed;
}

/*
  return the center stick position expressed as a control_in value
  used for thr_mid in copter
 */
int16_t rc_get_control_mid(rc_channel_t rc)
{
    if (rc->type_in == RC_CONTROL_TYPE_RANGE) {
        int16_t r_in = (rc->radio_min + rc->radio_max)/2;

        int16_t radio_trim_low  = rc->radio_min + rc->dead_zone;

        return (((int32_t)(rc->high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(rc->radio_max - radio_trim_low));
    } else {
        return 0;
    }
}

// calculate an angle given dead_zone and trim. This is used by the quadplane code
// for hover throttle
int16_t rc_pwm_to_angle_dz_trim(rc_channel_t rc, uint16_t _dead_zone, uint16_t _trim)
{
    int16_t radio_trim_high = _trim + _dead_zone;
    int16_t radio_trim_low  = _trim - _dead_zone;

    int16_t reverse_mul = (rc->reversed?-1:1);

    // don't allow out of range values
    int16_t r_in = math_constrain_int16(rc->radio_in, rc->radio_min, rc->radio_max);

    if (r_in > radio_trim_high && rc->radio_max != radio_trim_high) {
        return reverse_mul * ((int32_t)rc->high_in * (int32_t)(r_in - radio_trim_high)) / (int32_t)(rc->radio_max  - radio_trim_high);
    } else if (r_in < radio_trim_low && radio_trim_low != rc->radio_min) {
        return reverse_mul * ((int32_t)rc->high_in * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_trim_low - rc->radio_min);
    } else {
        return 0;
    }
}

// return a normalised input for a channel, in range -1 to 1,
// centered around the channel trim. Ignore deadzone.
float rc_norm_input(rc_channel_t rc)
{
    float ret;
    int16_t reverse_mul = (rc->reversed?-1:1);
    if (rc->radio_in < rc->radio_trim) {
        if (rc->radio_min >= rc->radio_trim) {
            return 0.0f;
        }
        ret = reverse_mul * (float)(rc->radio_in - rc->radio_trim) / (float)(rc->radio_trim - rc->radio_min);
    } else {
        if (rc->radio_max <= rc->radio_trim) {
            return 0.0f;
        }
        ret = reverse_mul * (float)(rc->radio_in - rc->radio_trim) / (float)(rc->radio_max  - rc->radio_trim);
    }
    return math_constrain_float(ret, -1.0f, 1.0f);
}

// return a normalised input for a channel, in range -1 to 1,
// centered around the channel trim. Take into account the deadzone
float rc_norm_input_dz(rc_channel_t rc)
{
    int16_t dz_min = rc->radio_trim - rc->dead_zone;
    int16_t dz_max = rc->radio_trim + rc->dead_zone;
    float ret;
    int16_t reverse_mul = (rc->reversed?-1:1);
    if (rc->radio_in < dz_min && dz_min > rc->radio_min) {
        ret = reverse_mul * (float)(rc->radio_in - dz_min) / (float)(dz_min - rc->radio_min);
    } else if (rc->radio_in > dz_max && rc->radio_max > dz_max) {
        ret = reverse_mul * (float)(rc->radio_in - dz_max) / (float)(rc->radio_max  - dz_max);
    } else {
        ret = 0;
    }
    return math_constrain_float(ret, -1.0f, 1.0f);
}

// return a normalised input for a channel, in range -1 to 1,
// ignores trim and deadzone
float rc_norm_input_ignore_trim(rc_channel_t rc)
{
    // sanity check min and max to avoid divide by zero
    if (rc->radio_max <= rc->radio_min) {
        return 0.0f;
    }
    const float ret = (rc->reversed ? -2.0f : 2.0f) * (((float)(rc->radio_in - rc->radio_min) / (float)(rc->radio_max - rc->radio_min)) - 0.5f);
    return math_constrain_float(ret, -1.0f, 1.0f);
}

uint8_t rc_percent_input(rc_channel_t rc)
{
    if (rc->radio_in <= rc->radio_min) {
        return rc->reversed?100:0;
    }
    if (rc->radio_in >= rc->radio_max) {
        return rc->reversed?0:100;
    }
    uint8_t ret = 100.0f * (rc->radio_in - rc->radio_min) / (float)(rc->radio_max - rc->radio_min);
    if (rc->reversed) {
        ret = 100 - ret;
    }
    return ret;
}

/*
  return true if input is within deadzone of trim
*/
bool rc_in_trim_dz(rc_channel_t rc)
{
    return is_bounded_int32(rc->radio_in, rc->radio_trim - rc->dead_zone, rc->radio_trim + rc->dead_zone);
}

void rc_clear_override(rc_channel_t rc)
{
    rc->last_override_time = 0;
    rc->override_value = 0;
}

void rc_set_override(rc_channel_t rc, const uint16_t v, const uint32_t timestamp_ms)
{
    if (!rcs_gcs_overrides_enabled()) {
        return;
    }

    rc->last_override_time = timestamp_ms != 0 ? timestamp_ms : time_millis();
    rc->override_value = v;
    rcs_new_override_received();
}

bool rc_has_override(rc_channel_const_t rc)
{
    if (rc->override_value == 0) {
        return false;
    }

    uint32_t override_timeout_ms;
    if (!rcs_get_override_timeout_ms(&override_timeout_ms)) {
        // timeouts are disabled
        return true;
    }

    if (override_timeout_ms == 0) {
        // overrides are explicitly disabled by a zero value
        return false;
    }

    return (time_millis() - rc->last_override_time < override_timeout_ms);
}

/*
  perform stick mixing on one channel
  This type of stick mixing reduces the influence of the auto
  controller as it increases the influence of the users stick input,
  allowing the user full deflection if needed
 */
float rc_stick_mixing(rc_channel_t rc, const float servo_in)
{
    float ch_inf = (float)(rc->radio_in - rc->radio_trim);
    ch_inf = fabsf(ch_inf);
    ch_inf = MIN(ch_inf, 400.0f);
    ch_inf = ((400.0f - ch_inf) / 400.0f);

    float servo_out = servo_in;
    servo_out *= ch_inf;
    servo_out += rc->control_in;

    return servo_out;
}

// get control input with zero deadzone
int16_t rc_get_control_in_zero_dz(rc_channel_t rc)
{
    if (rc->type_in == RC_CONTROL_TYPE_RANGE) {
        return rc_pwm_to_range_dz(rc, 0);
    }
    return rc_pwm_to_angle_dz(rc, 0);
}

/*
  convert a pulse width modulation value to a value in the configured
  range, using the specified deadzone
 */
int16_t rc_pwm_to_range_dz(rc_channel_t rc, uint16_t _dead_zone)
{
    int16_t r_in = math_constrain_int16(rc->radio_in, rc->radio_min, rc->radio_max);

    if (rc->reversed) {
        r_in = rc->radio_max - (r_in - rc->radio_min);
    }

    int16_t radio_trim_low  = rc->radio_min + _dead_zone;

    if (r_in > radio_trim_low) {
        return (((int32_t)(rc->high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(rc->radio_max - radio_trim_low));
    }
    return 0;
}

void rc_init_aux(rc_channel_t rc)
{
    enum RcAuxSwitchPos position;
    if (!rc_read_3pos_switch(rc, &position)) {
        position = RC_AUX_SWITCH_LOW;
    }

    rc_init_aux_function(rc, (rc_aux_func_t)rc->option, position);
}

/*
  read an aux channel. Return true if a switch has changed
 */
bool rc_read_aux(rc_channel_t rc)
{
    const rc_aux_func_t _option = (rc_aux_func_t)rc->option;
    if (_option == RC_AUX_DO_NOTHING) {
        // may wish to add special cases for other "AUXSW" things
        // here e.g. RCMAP_ROLL etc once they become options
        return false;
    } else if (_option == RC_AUX_VTX_POWER) {
        int8_t position;
        if (rc_read_6pos_switch(rc, &position)) {
            //AP::vtx().change_power(position);
            return true;
        }
        return false;
    }

    enum RcAuxSwitchPos new_position;
    if (!rc_read_3pos_switch(rc, &new_position)) {
        return false;
    }

    if (!rc_debounce_completed(rc, (int8_t)new_position)) {
        return false;
    }

    // debounced; undertake the action:
    rc_run_aux_function(rc, _option, new_position, RC_FUNC_TRIGGER_SOURCE_RC);
    return true;
}

// read_3pos_switch
bool rc_read_3pos_switch(rc_channel_const_t rc, enum RcAuxSwitchPos *ret)
{
    const uint16_t in = rc_get_radio_in(rc);
    if (in <= RC_MIN_LIMIT_PWM || in >= RC_MAX_LIMIT_PWM) {
        return false;
    }
    
    // switch is reversed if 'reversed' option set on channel and switches reverse is allowed by RC_OPTIONS
    bool switch_reversed = rc->reversed && rcs_switch_reverse_allowed();
    
    if (in < AUX_SWITCH_PWM_TRIGGER_LOW) {
        *ret = switch_reversed ? RC_AUX_SWITCH_HIGH : RC_AUX_SWITCH_LOW;
    } else if (in > AUX_SWITCH_PWM_TRIGGER_HIGH) {
        *ret = switch_reversed ? RC_AUX_SWITCH_LOW : RC_AUX_SWITCH_HIGH;
    } else {
        *ret = RC_AUX_SWITCH_MIDDLE;
    }
    return true;
}

// read a 6 position switch
bool rc_read_6pos_switch(rc_channel_t rc, int8_t* position)
{
    // calculate position of 6 pos switch
    const uint16_t pulsewidth = rc_get_radio_in(rc);
    if (pulsewidth <= RC_MIN_LIMIT_PWM || pulsewidth >= RC_MAX_LIMIT_PWM) {
        return false;  // This is an error condition
    }

    if      (pulsewidth < 1231) *position = 0;
    else if (pulsewidth < 1361) *position = 1;
    else if (pulsewidth < 1491) *position = 2;
    else if (pulsewidth < 1621) *position = 3;
    else if (pulsewidth < 1750) *position = 4;
    else *position = 5;

    if (!rc_debounce_completed(rc, *position)) {
        return false;
    }

    return true;
}


void rc_reset_mode_switch(rc_channel_t rc)
{
    rc->switch_state.current_position = -1;
    rc->switch_state.debounce_position = -1;
    rc_read_mode_switch(rc);
}

void rc_read_mode_switch(rc_channel_t rc)
{
    int8_t position;
    if (rc_read_6pos_switch(rc, &position)) {
        // set flight mode and simple mode setting
        rc_mode_switch_changed(rc, (modeswitch_pos_t)position);
    }
}

// 
bool rc_debounce_completed(rc_channel_t rc, int8_t position)
{
    // switch change not detected
    if (rc->switch_state.current_position == position) {
        // reset debouncing
        rc->switch_state.debounce_position = position;
    } else {
        // switch change detected
        const uint32_t tnow_ms = time_millis();

        // position not established yet
        if (rc->switch_state.debounce_position != position) {
            rc->switch_state.debounce_position = position;
            rc->switch_state.last_edge_time_ms = tnow_ms;
        } else if (tnow_ms - rc->switch_state.last_edge_time_ms >= SWITCH_DEBOUNCE_TIME_MS) {
            // position estabilished; debounce completed
            rc->switch_state.current_position = position;
            return true;
        }
    }

    return false;
}

//
// support for auxiliary switches:
//

bool rc_run_aux_function(rc_channel_t rc, rc_aux_func_t ch_option, enum RcAuxSwitchPos pos, enum RcAuxFuncTriggerSource source)
{
    const bool ret = rc_do_aux_function(rc, ch_option, pos);

#if 0
    // @LoggerMessage: AUXF
    // @Description: Auxiliary function invocation information
    // @Field: TimeUS: Time since system startup
    // @Field: function: ID of triggered function
    // @Field: pos: switch position when function triggered
    // @Field: source: source of auxiliary function invocation
    // @Field: result: true if function was successful
    AP::logger().Write(
        "AUXF",
        "TimeUS,function,pos,source,result",
        "s#---",
        "F----",
        "QHBBB",
        AP_HAL::micros64(),
        uint16_t(ch_option),
        uint8_t(pos),
        uint8_t(source),
        uint8_t(ret)
        );
#endif

    return ret;
}

// init_aux_switch_function - initialize aux functions
void rc_InitAuxFunction(rc_channel_t rc, const rc_aux_func_t ch_option, const enum RcAuxSwitchPos ch_flag)
{
    // init channel options
    switch(ch_option) {
    // the following functions do not need to be initialised:
    case RC_AUX_ARMDISARM:
    case RC_AUX_CAMERA_TRIGGER:
    case RC_AUX_CLEAR_WP:
    case RC_AUX_COMPASS_LEARN:
    case RC_AUX_DISARM:
    case RC_AUX_DO_NOTHING:
    case RC_AUX_LANDING_GEAR:
    case RC_AUX_LOST_VEHICLE_SOUND:
    case RC_AUX_RELAY:
    case RC_AUX_RELAY2:
    case RC_AUX_RELAY3:
    case RC_AUX_RELAY4:
    case RC_AUX_RELAY5:
    case RC_AUX_RELAY6:
    case RC_AUX_VISODOM_ALIGN:
    case RC_AUX_EKF_LANE_SWITCH:
    case RC_AUX_EKF_YAW_RESET:
    case RC_AUX_GENERATOR: // don't turn generator on or off initially
    case RC_AUX_EKF_POS_SOURCE:
    case RC_AUX_TORQEEDO_CLEAR_ERR:
    case RC_AUX_SCRIPTING_1:
    case RC_AUX_SCRIPTING_2:
    case RC_AUX_SCRIPTING_3:
    case RC_AUX_SCRIPTING_4:
    case RC_AUX_SCRIPTING_5:
    case RC_AUX_SCRIPTING_6:
    case RC_AUX_SCRIPTING_7:
    case RC_AUX_SCRIPTING_8:
    case RC_AUX_VTX_POWER:
    case RC_AUX_OPTFLOW_CAL:
    case RC_AUX_TURBINE_START:
    case RC_AUX_MOUNT1_ROLL:
    case RC_AUX_MOUNT1_PITCH:
    case RC_AUX_MOUNT1_YAW:
    case RC_AUX_MOUNT2_ROLL:
    case RC_AUX_MOUNT2_PITCH:
    case RC_AUX_MOUNT2_YAW:
        break;
    case RC_AUX_AVOID_ADSB:
    case RC_AUX_AVOID_PROXIMITY:
    case RC_AUX_FENCE:
    case RC_AUX_GPS_DISABLE:
    case RC_AUX_GPS_DISABLE_YAW:
    case RC_AUX_KILL_IMU1:
    case RC_AUX_KILL_IMU2:
    case RC_AUX_MISSION_RESET:
    case RC_AUX_MOTOR_ESTOP:
    case RC_AUX_RC_OVERRIDE_ENABLE:
    case RC_AUX_RUNCAM_CONTROL:
    case RC_AUX_RUNCAM_OSD_CONTROL:
    case RC_AUX_SPRAYER:
    case RC_AUX_DISABLE_AIRSPEED_USE:
    case RC_AUX_FFT_NOTCH_TUNE:
    case RC_AUX_LOG_PAUSE:
    case RC_AUX_ARM_EMERGENCY_STOP:
    case RC_AUX_CAMERA_REC_VIDEO:
    case RC_AUX_CAMERA_ZOOM:
    case RC_AUX_CAMERA_MANUAL_FOCUS:
    case RC_AUX_CAMERA_AUTO_FOCUS:
        rc_run_aux_function(rc, ch_option, ch_flag, RC_FUNC_TRIGGER_SOURCE_INIT);
        break;
    default:
        gcs_send_text(MAV_SEVERITY_WARNING, "Failed to init: RC%u_OPTION: %u\n",
                           (unsigned)(rc->ch_in+1), (unsigned)ch_option);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        brd_config_error("Failed to init: RC%u_OPTION: %u",
                           (unsigned)(rc->ch_in+1), (unsigned)ch_option);
#endif
        break;
    }
}

bool rc_DoAuxFunction(rc_channel_t rc, const rc_aux_func_t ch_option, const enum RcAuxSwitchPos ch_flag)
{
    switch(ch_option) {
    case RC_AUX_RC_OVERRIDE_ENABLE:
        // Allow or disallow RC_Override
        rc_do_aux_function_rc_override_enable(ch_flag);
        break;

    case RC_AUX_CLEAR_WP:
        rc_do_aux_function_clear_wp(ch_flag);
        break;
    case RC_AUX_MISSION_RESET:
        rc_do_aux_function_mission_reset(ch_flag);
        break;

    case RC_AUX_ARMDISARM:
        rc_do_aux_function_armdisarm(ch_flag);
        break;

    case RC_AUX_DISARM:
        if (ch_flag == RC_AUX_SWITCH_HIGH) {
            mb_arming_disarm(ARMING_METHOD_AUXSWITCH, true);
        }
        break;

    case RC_AUX_MOTOR_ESTOP:
        switch (ch_flag) {
        case RC_AUX_SWITCH_HIGH: {
            srv_channels_set_emergency_stop(true);
            break;
        }
        case RC_AUX_SWITCH_MIDDLE:
            // nothing
            break;
        case RC_AUX_SWITCH_LOW: {
            srv_channels_set_emergency_stop(false);
            break;
        }
        }
        break;

#if 0

#if !HAL_MINIMIZE_FEATURES
    case RC_AUX_KILL_IMU1:
        sensor_imu_kill_imu(0, ch_flag == AuxSwitchPos::HIGH);
        break;

    case RC_AUX_KILL_IMU2:
        sensor_imu_kill_imu(1, ch_flag == AuxSwitchPos::HIGH);
        break;
#endif // HAL_MINIMIZE_FEATURES



    case RC_AUX_ARM_EMERGENCY_STOP: {
        switch (ch_flag) {
           case AuxSwitchPos::HIGH:
               // request arm, disable emergency motor stop
               SRV_Channels::set_emergency_stop(false);
               AP::arming().arm(AP_Arming::Method::AUXSWITCH, true);
               break;
           case AuxSwitchPos::MIDDLE:
               // disable emergency motor stop
               SRV_Channels::set_emergency_stop(false);
               break;
           case AuxSwitchPos::LOW:
               // enable emergency motor stop
               SRV_Channels::set_emergency_stop(true);
               break;
        }
        break;
    }
#endif
    // do nothing for these functions
    case RC_AUX_MOUNT1_ROLL:
    case RC_AUX_MOUNT1_PITCH:
    case RC_AUX_MOUNT1_YAW:
    case RC_AUX_MOUNT2_ROLL:
    case RC_AUX_MOUNT2_PITCH:
    case RC_AUX_MOUNT2_YAW:
    case RC_AUX_SCRIPTING_1:
    case RC_AUX_SCRIPTING_2:
    case RC_AUX_SCRIPTING_3:
    case RC_AUX_SCRIPTING_4:
    case RC_AUX_SCRIPTING_5:
    case RC_AUX_SCRIPTING_6:
    case RC_AUX_SCRIPTING_7:
    case RC_AUX_SCRIPTING_8:
        break;

    default:
        gcs_send_text(MAV_SEVERITY_INFO, "Invalid channel option (%u)", (unsigned int)ch_option);
        return false;
    }

    return true;
}

void rc_do_aux_function_clear_wp(const enum RcAuxSwitchPos ch_flag)
{
    if (ch_flag == RC_AUX_SWITCH_HIGH) {
        mission_clear();
    }
}

void rc_do_aux_function_rc_override_enable(const enum RcAuxSwitchPos ch_flag)
{
    switch (ch_flag) {
    case RC_AUX_SWITCH_HIGH: {
        rcs_set_gcs_overrides_enabled(true);
        break;
    }
    case RC_AUX_SWITCH_MIDDLE:
        // nothing
        break;
    case RC_AUX_SWITCH_LOW: {
        rcs_set_gcs_overrides_enabled(false);
        break;
    }
    }
}

void rc_do_aux_function_mission_reset(const enum RcAuxSwitchPos ch_flag)
{
    if (ch_flag != RC_AUX_SWITCH_HIGH) {
        return;
    }

    mission_reset();
}

void rc_do_aux_function_armdisarm(const enum RcAuxSwitchPos ch_flag)
{
    // arm or disarm the vehicle
    switch (ch_flag) {
    case RC_AUX_SWITCH_HIGH:
        mb_arming_arm(ARMING_METHOD_AUXSWITCH, true);
        break;
    case RC_AUX_SWITCH_MIDDLE:
        // nothing
        break;
    case RC_AUX_SWITCH_LOW:
        mb_arming_disarm(ARMING_METHOD_AUXSWITCH, true);
        break;
    }
}


// return switch position value as LOW, MIDDLE, HIGH
// if reading the switch fails then it returns LOW
enum RcAuxSwitchPos rc_get_aux_switch_pos(rc_channel_const_t rc)
{
    enum RcAuxSwitchPos position = RC_AUX_SWITCH_LOW;
    (void)rc_read_3pos_switch(rc, &position);

    return position;
}

/*
  Return true if value is between lower and upper bound inclusive.
  False otherwise.
*/
static bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound)
{
    if ((lower_bound <= upper_bound) &&
        (value >= lower_bound) && (value <= upper_bound)) {
        return true;
    }

    return false;
}

/*------------------------------------test------------------------------------*/


